/*
 * slam.cc
 * Based on joystick.cc, performs feature based slam in
 * landmarks.cfg, as described in Probabilistic Robotics.
 */

#include <slam.h>

using namespace PlayerCc;

int main(int argc, char **argv){
    
    try{
        
        // this structure is maintained by the joystick reader
        memset( &cont, 0, sizeof(cont) );
        
        pthread_t dummy;
        
        if(!strlen(jsdev))
            jfd = open (DEFAULT_DEV, O_RDONLY);
        else
            jfd = open (jsdev, O_RDONLY);
        
        if(jfd < 1){
            perror("Failed to open joystick");
            //fprintf(stderr, "Close application.");
        }
        else
            pthread_create(&dummy, NULL, &joystick_handler, (void*)&cont);
        //end of joystick read structure
        
        // create structures for kalman filter based SLAM
        double dist_blob[12] = {8,8,8,8,8,8,8,8,8,8,8,8};
        double theta_blob[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
        int signature_blob[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
        
        list<lista_mapa> mapa;
        list<lista_sensor> sensor;
        
        double controle[2] = {0,0};
        double t = 0;
        double t_old = 0;
        double d_t = 0;
        
        PGMATRIX Matriz_Mediat;
        Matriz_Mediat = PGMATRIX_ALLOC(3,1);
        PGMATRIX_ZEROES(Matriz_Mediat);
        PGMATRIX_DATA(Matriz_Mediat, 1, 1) = 10;

        PGMATRIX Matriz_Covt;
        Matriz_Covt = PGMATRIX_ALLOC(39,39);
        PGMATRIX_IDENTITY(Matriz_Covt); // NEW STUFF
        PGMATRIX_MULTIPLY_CONST(Matriz_Covt,10^52);
        PGMATRIX_DATA(Matriz_Covt, 1 , 1) = 0;
        PGMATRIX_DATA(Matriz_Covt, 2 , 1) = 0;
        PGMATRIX_DATA(Matriz_Covt, 3 , 1) = 0;
        
        Inicializa_Lista_Mapa(&mapa);
        Inicializa_Lista_Sensor(&sensor);
        
        // connect to robot and relevant devices
        PlayerClient robot("localhost", 6665);
        Position2dProxy pp(&robot,0);
        LaserProxy lp(&robot,0);
    	BlobfinderProxy bp(&robot,0);
    	FiducialProxy fp(&robot,0);
    	SimulationProxy sp(&robot,0);
        
        pp.SetMotorEnable (true);
        robot.Read();

        int k=0;

        while(true){
        
            printf("loop #%d\n",k++);
            
            robot.Read();
            t = lp.GetDataTime();
            d_t = t - t_old;
            
            if(button[0] || (jfd < 1)){
            
    		    //printf("\n(Number of blobs: %d)\n\n",countBlobs(&bp));
    		    
    		    for(unsigned int i=0; i < bp.GetCount(); i++)
    		        if(getLandmarkID(bp[i].color)){
    		        
    		            /*std::cout << "Landmark[" << getLandmarkID(bp[i].color) << "] at " << std::endl;
    		            std::cout << "\t" << "rho=" << getLandmarkRange(&lp,&bp,i) << std::endl;
    		            std::cout << "\t" << "phi=" << getLandmarkBearing(&lp,&bp,i) << std::endl;
    		            std::cout << "\t" << "sig=" << getLandmarkSignature(&bp,i) << std::endl;*/
    		            
    		        }
                    
    	    }
            
            //if joystick has new info, update speed and turnrate
            if(cont.dirty || jfd < 1){
                //button[10] handles the enabling/disabling of rear gear
                if(button[10] && cont.speed < 0)
                    cont.speed=0;
    			
    			if((lp[180] < 1) && (jfd < 1))
    			    controle[0] = 0.1;
    			else if (jfd < 1)
    			    controle[0] = 2;
    			    
    			if((lp[0] > 1) && (lp[361] > 1) && (jfd < 1))
    			    controle[1] = 0;    
    			else if((lp[0] > lp[361]) && (jfd < 1))
    			    controle[1] = -4;
    			else if ((lp[0] < lp[361]) && (jfd < 1))
    			    controle[1] = 4;
    			else if (jfd < 1)
    			    controle[1] = 0;
    			
    			if(jfd < 1)
                    pp.SetSpeed(NORMALIZE_SPEED(controle[0]), NORMALIZE_TURN(controle[1]));
                else{
                    pp.SetSpeed(NORMALIZE_SPEED(cont.speed), NORMALIZE_TURN(cont.turnrate));
                    controle[0] = NORMALIZE_SPEED(cont.speed);
                    controle[1] = NORMALIZE_TURN(cont.turnrate);
                }
                
                cont.dirty = false; // we've handled the changes
            }     
            
            if(button[11]){
                
                std::cout << "GOODBYE!" << std::endl;
                
                robot.Stop();
                
                return 0;
                
            }
            
            // SLAM data update (TESTING FROM HERE)
            for(unsigned int i=0; i < 12; i++)
                signature_blob[i] = 0;
            
            for(unsigned int i=0; i < bp.GetCount(); i++)
		        if(getLandmarkID(bp[i].color)){
		            
		            int index = getLandmarkID(bp[i].color)-1;
		                    		                   		            
		            dist_blob[index] = getLandmarkRange(&lp,&bp,i);
		            theta_blob[index] = getLandmarkBearing(&lp,&bp,i);	// Angulo em radianos
                    signature_blob[index] = 1;
		            
		        }

            /*std:: cout << "Sensores:\n";           
            for(int index=0; index < 12; index++)
                std:: cout << index << " " << dist_blob[index] << " " << theta_blob[index] << " " << signature_blob[index] << "\n";
            
            if(button[1] || jfd < 1){
            
                cout.width(5);
                std::cout << "EKF Estimated Pose" << std::endl << std::endl;
                std::cout << "X " << PGMATRIX_DATA(Matriz_Mediat, 1, 1) << " " << PGMATRIX_DATA(Matriz_Covt, 1, 1);
                std::cout << std::endl;
                std::cout << "Y " << PGMATRIX_DATA(Matriz_Mediat, 2, 1) << " " << PGMATRIX_DATA(Matriz_Covt, 2, 2);
                std::cout << std::endl;
                std::cout << "THETA " << RTOD(PGMATRIX_DATA(Matriz_Mediat, 3, 1))+90 << " " << PGMATRIX_DATA(Matriz_Covt, 3, 3);
                std::cout << std::endl << std::endl;
            
            }*/
            
            // SLAM usage
            Atualiza_Lista_Sensor(&sensor,dist_blob,theta_blob,signature_blob);
            SLAM(Matriz_Mediat, Matriz_Covt, controle, signature_blob, &mapa, &sensor, d_t);
            
            //PGMATRIX_DATA(Matriz_Mediat,1,1) = pp.GetXPos();
            //PGMATRIX_DATA(Matriz_Mediat,2,1) = pp.GetYPos();
            //PGMATRIX_DATA(Matriz_Mediat,3,1) = pp.GetYaw();
            
            //PGMATRIX_DATA(Matriz_Covt,1,1) = 10^-20;
            //PGMATRIX_DATA(Matriz_Covt,2,2) = 10^-20;
            //PGMATRIX_DATA(Matriz_Covt,3,3) = 10^-20;
            
            //Updates the estimated positions of the landmarks and the robot
            updateEstimatedRobot(&sp, Matriz_Mediat);
            updateEstimatedLandmarks(&sp, signature_blob, &mapa);
            
            t_old = t;
            
        }
        
        return 0;
        
    }
    catch(PlayerCc::PlayerError e){
        std::cerr << e << std::endl;
        return -1;
    };
    
}

//This function returns the number of blobs the robot can see
int countBlobs(PlayerCc::BlobfinderProxy *bp){

	int number = (*bp).GetCount();
    
    for(unsigned int i=0; i < (*bp).GetCount(); i++)
        if((*bp)[i].color == GRAY)
            number--;
	
	return number;
}

//This function returns the Landmark ID of a blob, given its color
int getLandmarkID(int color){

    switch(color){
    
        case RED:               return 1;
        case BLUE:              return 2;
        case GREEN:             return 3;
        case CYAN:              return 4;
        case YELLOW:            return 5;
        case MAGENTA:           return 6;
        case LIGHTSTEELBLUE:    return 7;
        case SEAGREEN:          return 8;
        case GOLDENROD:         return 9;
        case CHOCOLATE:         return 10;
        case PEACHPUFF1:        return 11;
        case OLIVEDRAB1:        return 12;
        default:                return 0;
    
    }

}

//This function searches for a local minima in laser ranges, given a reference angle provided by the blobfinder
double getLandmarkRange(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index){

    double l_radius=0.2;
    double lp_range;
    double range = MAX_RANGE;
    int start = START;
    int end = END;
    
    for(int i=start; i <= end; i++){
    
        lp_range = (*lp)[i];
        
        range = (range < lp_range) ? range : lp_range;
        
    }
    
    return range+l_radius; 

}

//This function searches for a local minima in laser ranges, given a reference angle provided by the blobfinder
double getLandmarkBearing(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index){

    double lp_range;
    double range = MAX_RANGE;
    double angle = 0;
    
    int start = START;
    int end = END;
    
    for(int i=start; i <= end; i++){
    
        lp_range = (*lp)[i];
    
        if(range > lp_range){
        
            range = lp_range;
            angle = (double) i/2;
        
        }
        
    }
        
    return DTOR(angle)-DTOR(90); 

}

//This function returns the signature of a given landmark
const char* getLandmarkSignature(PlayerCc::BlobfinderProxy *bp, int blob_index){

    int color = (*bp)[blob_index].color;

    switch(color){
    
        case RED:               return "RED";
        case BLUE:              return "BLUE";
        case GREEN:             return "GREEN";
        case CYAN:              return "CYAN";
        case YELLOW:            return "YELLOW";
        case MAGENTA:           return "MAGENTA";
        case LIGHTSTEELBLUE:    return "LIGHTSTEELBLUE";
        case SEAGREEN:          return "SEAGREEN";
        case GOLDENROD:         return "GOLDENROD";
        case CHOCOLATE:         return "CHOCOLATE";
        case PEACHPUFF1:        return "PEACHPUFF1";
        case OLIVEDRAB1:        return "OLIVEDRAB1";
        default:                return "UNKNOWN";
    
    }

}

//This function updates the estimated position of the landmarks
void updateEstimatedLandmarks(PlayerCc::SimulationProxy *sp, int *signature_blob, list<lista_mapa> *lista_mapa){

    double positions[3];
    char str[10];

    for (unsigned int i = 0; i < 12; i++){
    
        if (signature_blob[i]){
        
            sprintf(str, "e_lm%d", i+1);         
            i = Ler_Lista_Mapa(lista_mapa, i, positions);
            sp->SetPose2d(str, positions[0]-20, positions[1] ,0);
            //printf("%s",str);
            //printf(" at (x,y)=(%lf,%lf)\n\n",positions[0], positions[1]);
            
        }
        
    }
    
}

//This function updates the estimated position of the robot
void updateEstimatedRobot(PlayerCc::SimulationProxy *sp, PGMATRIX Position){
    
    //printf("Placing robot2 at (X,Y,THETA)=(%lf,%lf,%lf)\n\n",PGMATRIX_DATA(Position, 1, 1),PGMATRIX_DATA(Position, 2, 1),PGMATRIX_DATA(Position, 3, 1));
    sp->SetPose2d((char*)"robot2", PGMATRIX_DATA(Position, 1, 1)-20, PGMATRIX_DATA(Position, 2, 1), PGMATRIX_DATA(Position, 3, 1));
    
}

//compile this program by inserting the following command in terminal while in its directory:
//g++ -o localization `pkg-config --cflags playerc++` localization.cc `pkg-config --libs playerc++`
